#include "driver/i2c.h"
#include "esp_log.h"
#include "_i2c.h"
#include "zyp_log.h"
// #include "driver/gpio.h"
// #include "gpio_types.h"

QueueHandle_t ina219_queue;


// 写入16位寄存器
esp_err_t ina219_write_register_third(i2c_port_t i2c_num, uint8_t device_addr, uint8_t reg, uint16_t value)
{
    // 假设你的 INA219 地址是 0x40（二进制 1000000）
    // 注意：实际地址可能因 ADDR 引脚接法不同而变化
    // uint8_t device_addr = 0x48;  // 根据硬件修改：0x40~0x4F

    uint8_t data[3] = {reg, (uint8_t)(value >> 8), (uint8_t)(value & 0xFF)};

    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);

    i2c_master_write_byte(cmd, (device_addr << 1) | I2C_MASTER_WRITE, true);

    i2c_master_write(cmd, data, sizeof(data), true);
    i2c_master_stop(cmd);

    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, pdMS_TO_TICKS(1000));
    i2c_cmd_link_delete(cmd);

    if (ret != ESP_OK)
    {
        ZYP_DEBUG("I2C write failed: %s\n", esp_err_to_name(ret));
    }
    else
    {
        ZYP_DEBUG("ina219_write_register_third done, 00 reg is INA219_CONFIG scuess \n");
    }

    return ret;
}

// 读取16位寄存器
esp_err_t ina219_read_register_third(i2c_port_t i2c_num, uint8_t dev_addr, uint8_t reg, uint16_t *value)
{
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (dev_addr << 1) | I2C_MASTER_WRITE, true);

    i2c_master_write_byte(cmd, reg, true);

    i2c_master_start(cmd); // 重复启动
    i2c_master_write_byte(cmd, (dev_addr << 1) | I2C_MASTER_READ, true);

    uint8_t msb, lsb;
    i2c_master_read_byte(cmd, &msb, I2C_MASTER_ACK);
    i2c_master_read_byte(cmd, &lsb, I2C_MASTER_LAST_NACK);
    i2c_master_stop(cmd);

    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, pdMS_TO_TICKS(1000));
    i2c_cmd_link_delete(cmd);

    if (ret == ESP_OK)
    {
        *value = (msb << 8) | lsb;
    }
    return ret;
}

// 初始化INA219[7](@ref)
esp_err_t ina219_init_third(i2c_port_t i2c_num, uint8_t device_addr)
{
    esp_err_t ret;
    uint16_t reg_value;

    // read reg INA219_CONFIG defalut value
    ret = ina219_read_register_third(i2c_num, device_addr, INA219_CONFIG, &reg_value);
    if (ret == ESP_OK)
    {
        ZYP_DEBUG("device_addr:%02x  Before write - INA219_CONFIG (0x00) register value: 0x%04X\n", device_addr, reg_value);
    }
    else
    {
        ZYP_DEBUG("device_addr:%02x  Failed to read INA219_CONFIG register before write: %s\n", device_addr, esp_err_to_name(ret));
    }
    // write reg INA219_CONFIG  value
    ret = ina219_write_register_third(i2c_num, device_addr, INA219_CONFIG, CONFIG_VALUE);
    if (ret != ESP_OK)
    {
        ZYP_DEBUG("%d\n", __LINE__);
        ZYP_DEBUG("ERR %s\n", esp_err_to_name(ret));
        return ret;
    }
    else
    {
        ZYP_DEBUG("device_addr:%02x   INA219_write_register: 0x00 INA219_CONFIG OK \n", device_addr);
    }
    // verify again read reg INA219_CONFIG
    ret = ina219_read_register_third(i2c_num, device_addr, INA219_CONFIG, &reg_value);
    if (ret == ESP_OK)
    {
        ZYP_DEBUG("After write - INA219_CONFIG (0x00) register value: 0x%04X\n", reg_value);
    }
    else
    {
        ZYP_DEBUG("Failed to read INA219_CONFIG register after write: %s\n", esp_err_to_name(ret));
    }



    //  read reg INA219_CALIBRATION defalut value
    ret = ina219_read_register_third(i2c_num, device_addr, INA219_CALIBRATION, &reg_value);
    if (ret == ESP_OK)
    {
        ZYP_DEBUG("device_addr:%02x  Before write - INA219_CALIBRATION (0x05) register value: 0x%04X\n", device_addr, reg_value);
    }
    else
    {
        ZYP_DEBUG("device_addr:%02x  Failed to read INA219_CALIBRATION register before write: %s\n", device_addr, esp_err_to_name(ret));
    }

    //  write reg INA219_CONFIG  value
    ret = ina219_write_register_third(i2c_num, device_addr, INA219_CALIBRATION, CALIBRATION_VALUE);
    if (ret != ESP_OK)
    {
        ZYP_DEBUG("%d\n", __LINE__);
        ZYP_DEBUG("ERR %s\n", esp_err_to_name(ret));
    }
    else
    {
        ZYP_DEBUG("INA219_write_register: 0x05 INA219_CALIBRATION OK \n");
    }
    // verify again read reg INA219_CONFIG
    ret = ina219_read_register_third(i2c_num, device_addr, INA219_CALIBRATION, &reg_value);
    if (ret == ESP_OK)
    {
        ZYP_DEBUG("  After write - INA219_CALIBRATION (0x05) register value: 0x%04X\n", reg_value);
    }
    else
    {
        ZYP_DEBUG("Failed to read INA219_CALIBRATION register after write: %s\n", esp_err_to_name(ret));
    }

    return ESP_OK;
}

// read voltage(mV)
esp_err_t ina219_get_bus_voltage_third(i2c_port_t i2c_num, uint8_t dev_addr, uint16_t *voltage)
{
    uint16_t raw;
    esp_err_t ret = ina219_read_register_third(i2c_num, dev_addr, INA219_BUS_VOLTAGE, &raw); /// slave addr 40 写死
    if (ret == ESP_OK)
    {
        *voltage = (raw >> 3) * 4; // 每LSB=4mV[7](@ref)
    }
    else
    {
        *voltage = 0;
    }
    return ret;
}

// read current(mA)
esp_err_t ina219_get_current_third(i2c_port_t i2c_num, uint8_t dev_addr, uint16_t *current)
{
    uint16_t raw;
    esp_err_t ret = ina219_read_register_third(i2c_num, dev_addr, INA219_CURRENT, &raw);
    if (ret == ESP_OK)
    {
        // 处理符号位(有符号16位值)
        int16_t signed_raw = (int16_t)raw;
        *current = signed_raw * CURRENT_LSB * 1000; // 转换为mA
    }
    return ret;
}

// read power(mW)
esp_err_t ina219_get_power_third(i2c_port_t i2c_num, uint8_t dev_addr, uint16_t *power)
{
    uint16_t raw;
    esp_err_t ret = ina219_read_register_third(i2c_num, dev_addr, INA219_POWER, &raw);
    if (ret == ESP_OK)
    {
        *power = raw * POWER_LSB * 1000; // 转换为mW
    }
    return ret;
}

/**
 * @brief 读取原始寄存器值（16位）
 * @param reg_addr 寄存器地址
 * @return 原始值 (int16_t)
 */
int16_t ina219_read_raw(i2c_port_t i2c_num, uint8_t dev_addr, uint8_t reg_addr)
{
    uint8_t raw_data[2];
    esp_err_t ret = zyp_i2c_read_register_one(i2c_num, dev_addr, reg_addr, raw_data, 2);
    return (ret == ESP_OK) ? (int16_t)((raw_data[0] << 8) | raw_data[1]) : 0;
}

// // 物理量读取函数
// float ina219_read_bus_voltage_v(i2c_port_t i2c_num,uint8_t dev_addr) {
//     int16_t raw = ina219_read_raw(i2c_num,dev_addr, INA219_BUS_VOLTAGE);      ///总线电压值 = 寄存器值>>3 * LSB(4mV) 上图可知数据存放于寄存器的 3-15Bit ，故计算的话先右移三位去掉其余非数据位，再乘最小分辨率LSB(4mV)
//     return (raw >> 3) * 0.004f;  // 每LSB=4mV [2](@ref)
// }

// float ina219_read_current_ma(i2c_port_t i2c_num,uint8_t dev_addr) {
//     int16_t raw = ina219_read_raw(i2c_num,dev_addr, INA219_CURRENT);        ///电流值 = 读取到的寄存器值 * Current_LSB ，Current_LSB 的计算参考第6小节说明
//     return raw * g_currentLSB * 1000.0f;  // 转换为mA
// }

// float ina219_read_power_mw(i2c_port_t i2c_num,uint8_t dev_addr) {
//     int16_t raw = ina219_read_raw(i2c_num,dev_addr, INA219_POWER);
//     return raw * 20 * g_currentLSB * 1000.0f;  // 功率=电流*总线电压 [1](@ref)              ///功率 = 寄存器值 * Power_LSB ，Power_LSB 的计算参考第6小节说明
// }

// float ina219_read_shunt_voltage_mv(i2c_port_t i2c_num,uint8_t dev_addr) {
//     int16_t raw = ina219_read_raw(i2c_num,dev_addr, INA219_SHUNT_VOLTAGE);
//     return raw * 0.01f;  // 每LSB=10μV [3](@ref)
// }

// INA219写入寄存器函数
esp_err_t ina219_write_register(i2c_port_t i2c_num, uint8_t dev_addr, uint8_t reg, uint16_t value)
{
    uint8_t data[3] = {reg, (uint8_t)(value >> 8), (uint8_t)(value & 0xFF)};

    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (dev_addr << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write(cmd, data, sizeof(data), true);
    i2c_master_stop(cmd);

    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, pdMS_TO_TICKS(1000));
    i2c_cmd_link_delete(cmd);
    return ret;
}

// 修正ina219_configure函数
esp_err_t ina219_configure(i2c_port_t i2c_num, uint8_t dev_addr, float maxVoltage, float maxCurrent, float shuntResistor)
{
    // 1. 计算配置寄存器值
    uint16_t config = INA219_CONFIG_RESET | INA219_CONFIG_BVOLTAGE_16V |
                      INA219_CONFIG_GAIN_80MV | INA219_CONFIG_BADC_12BIT |
                      INA219_CONFIG_SADC_12BIT | INA219_CONFIG_MODE_CONT;

    // 2. 计算校准参数
    g_currentLSB = maxCurrent / 32768.0f;
    g_calibrationValue = (uint16_t)(0.04096f / (g_currentLSB * shuntResistor));

    // 3. 写入配置寄存器
    esp_err_t ret = ina219_write_register(i2c_num, dev_addr, INA219_CONFIG, config);
    if (ret != ESP_OK)
    {
        ESP_LOGE("INA219", "Failed to write config register: %d", ret);
        return ret;
    }

    vTaskDelay(pdMS_TO_TICKS(10));

    // 4. 写入校准寄存器
    ret = ina219_write_register(i2c_num, dev_addr, INA219_CALIBRATION, g_calibrationValue);
    if (ret != ESP_OK)
    {
        ESP_LOGE("INA219", "Failed to write calibration register: %d", ret);
        return ret;
    }

    vTaskDelay(pdMS_TO_TICKS(10));
    return ESP_OK;
}

/// 6 s data

esp_err_t zyp_i2c_master_init(i2c_port_t i2c_num, int sda_io_num, int scl_io_num)
{
    if (sda_io_num == scl_io_num)
    {
        ZYP_DEBUG("sda_io_num == scl_io_num\n");
        return ESP_FAIL;
    }

    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = sda_io_num, /// ZP_I2C_MASTER_SCL_IO
        .scl_io_num = scl_io_num, /// ZP_I2C_MASTER_SDA_IO
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = ZP_I2C_MASTER_FREQ_HZ,
        .clk_flags = 0, // Use the default clock source
    };

    // Configure the I2C parameters
    ESP_ERROR_CHECK(i2c_param_config(i2c_num, &conf)); ///

    // Install the I2C driver
    ESP_ERROR_CHECK(i2c_driver_install(i2c_num, conf.mode, 0, 0, 0));

    ZYP_DEBUG(" zyp_i2c_master_init end\n");
    return ESP_OK;
}

// esp_err_t zyp_test_i2c_read_register(i2c_port_t i2c_num1,int sda_io_num1,int scl_io_num1, i2c_port_t i2c_num2,int sda_io_num2,int scl_io_num2,uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, size_t len)
// {
//     zyp_i2c_master_init();

// }

esp_err_t zyp_i2c_read_register_one(i2c_port_t i2c_num, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, size_t len)
{
    if (len == 0)
    {
        return ESP_OK;
    }

    // Create an I2C command chain

    i2c_cmd_handle_t cmd = i2c_cmd_link_create(); /// construc i2c command channels
    if (cmd == NULL)
    {
        return ESP_ERR_NO_MEM;
    }

    // Construct the read sequence:
    // 1. Send the start signal
    // 2. Write slave device address (Write mode)
    // 3. Write the address of the register to be read
    // 4. Send a repeated start signal
    // 5. Write device address (read mode)
    // 6. Read the data
    // 7. Send a stop signal
    // ZYP_DEBUG("------i2c_master_start satrt-------\n");
    ESP_ERROR_CHECK(i2c_master_start(cmd));                                                /// construct start cmd
    ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (dev_addr << 1) | I2C_MASTER_WRITE, true)); /// choice the salve device address and write mode
    ESP_ERROR_CHECK(i2c_master_write_byte(cmd, reg_addr, true));                           /// choice the register address

    ESP_ERROR_CHECK(i2c_master_start(cmd)); /// switch to read mode avoid slave hold the bus
    ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (dev_addr << 1) | I2C_MASTER_READ, true));

    if (len > 1)
    {
        ESP_ERROR_CHECK(i2c_master_read(cmd, data, len - 1, I2C_MASTER_ACK));
    }
    ESP_ERROR_CHECK(i2c_master_read_byte(cmd, data + len - 1, I2C_MASTER_NACK));

    ESP_ERROR_CHECK(i2c_master_stop(cmd));

    // ZYP_DEBUG("------i2c_master_cmd_begin satrt-------\n");
    // Execute the I2C command
    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, 1000 / portTICK_PERIOD_MS); /// start i2c command channels
    // ZYP_DEBUG("------i2c_master_cmd_begin end-------\n");

    // Delete the command chain
    i2c_cmd_link_delete(cmd);

    return ret;
}

esp_err_t zyp_i2c_read_registe_two(uint8_t reg_addr, uint8_t *data, size_t slave_addr)
{
    esp_err_t ret;
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();

    // Send the register address to the slave machine
    i2c_master_write_byte(cmd, slave_addr << 1, true); // 写入设备地址，左移1位是为了包含读写位
    i2c_master_write_byte(cmd, reg_addr, true);        // 写入寄存器地址

    // Send the start signal and read the data
    i2c_master_start(cmd);
    i2c_master_read_byte(cmd, data, I2C_MASTER_LAST_NACK); // 读取数据

    // Send the stop signal
    i2c_master_stop(cmd);

    // Execute the I2C command
    ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd); // 释放命令句柄

    return ret;
}

void i2c_bus_diagnostic(i2c_port_t i2c_num, int sda_pin, int scl_pin)
{
    printf("\n=== I2C Bus %d Diagnostic ===\n", i2c_num);
    printf("SDA Pin: GPIO%d\n", sda_pin);
    printf("SCL Pin: GPIO%d\n", scl_pin);

    // 检查GPIO引脚配置
    gpio_config_t io_conf;
    io_conf.pin_bit_mask = (1ULL << sda_pin) | (1ULL << scl_pin);
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
    io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    io_conf.intr_type = GPIO_INTR_DISABLE;
    gpio_config(&io_conf);

    // 读取引脚状态
    int sda_state = gpio_get_level(sda_pin);
    int scl_state = gpio_get_level(scl_pin);

    printf("SDA line state: %s\n", sda_state ? "HIGH" : "LOW");
    printf("SCL line state: %s\n", scl_state ? "HIGH" : "LOW");

    if (sda_state == 0 || scl_state == 0)
    {
        printf("WARNING: SDA or SCL line is pulled LOW - check for bus contention or missing pull-up resistors\n");
    }

    // 重新配置引脚为I2C功能
    io_conf.pin_bit_mask = (1ULL << sda_pin) | (1ULL << scl_pin);
    io_conf.mode = GPIO_MODE_INPUT_OUTPUT_OD;
    io_conf.pull_up_en = GPIO_PULLUP_ENABLE;
    io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    io_conf.intr_type = GPIO_INTR_DISABLE;
    gpio_config(&io_conf);

    printf("================================\n\n");
}

// 增强版I2C扫描函数
void enhanced_i2c_scanner(i2c_port_t i2c_num)
{
    printf("\n=== Enhanced I2C Scanner for Bus %d ===\n", i2c_num);

    int found_count = 0;
    for (int addr = 0x08; addr < 0x78; addr++)
    {
        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
        i2c_master_start(cmd);
        i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
        i2c_master_stop(cmd);

        esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, pdMS_TO_TICKS(50));
        i2c_cmd_link_delete(cmd);

        if (ret == ESP_OK)
        {
            printf("Device found at address 0x%02X\n", addr);
            found_count++;
        }
        else if (ret != ESP_ERR_TIMEOUT)
        {
            printf("Error scanning address 0x%02X: %d\n", addr, ret);
        }
    }
    if (found_count == 0)
    {
        printf("No devices found on I2C bus %d\n", i2c_num);
        printf("Possible causes:\n");
        printf("1. I2C bus not properly initialized\n");
        printf("2. GPIO pins configured incorrectly\n");
        printf("3. No pull-up resistors on SDA/SCL lines\n");
        printf("4. Devices not powered or connected\n");
    }
    else
    {
        printf("Total devices found: %d\n", found_count);
    }
    printf("==========================================\n\n");
}

// 初始化I2C总线（增强版）
esp_err_t zyp_i2c_master_init_enhanced(i2c_port_t i2c_num, int sda_io_num, int scl_io_num)
{
    if (sda_io_num == scl_io_num)
    {
        ESP_LOGE("I2C", "sda_io_num == scl_io_num");
        return ESP_FAIL;
    }

    // 先卸载驱动（如果已安装）
    i2c_driver_delete(i2c_num);

    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = sda_io_num,
        .scl_io_num = scl_io_num,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = 100000, // 100kHz
        .clk_flags = 0,
    };

    // 配置I2C参数
    esp_err_t ret = i2c_param_config(i2c_num, &conf);
    if (ret != ESP_OK)
    {
        ESP_LOGE("I2C", "i2c_param_config failed: %d", ret);
        return ret;
    }

    // 安装I2C驱动
    ret = i2c_driver_install(i2c_num, conf.mode, 0, 0, 0);
    if (ret != ESP_OK)
    {
        ESP_LOGE("I2C", "i2c_driver_install failed: %d", ret);
        return ret;
    }

    ESP_LOGI("I2C", "I2C bus %d initialized successfully", i2c_num);
    return ESP_OK;
}

// INA219读取寄存器函数
esp_err_t ina219_read_register(i2c_port_t i2c_num, uint8_t dev_addr, uint8_t reg, uint16_t *value)
{
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (dev_addr << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, reg, true);

    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (dev_addr << 1) | I2C_MASTER_READ, true);

    uint8_t msb, lsb;
    i2c_master_read_byte(cmd, &msb, I2C_MASTER_ACK);
    i2c_master_read_byte(cmd, &lsb, I2C_MASTER_NACK);
    i2c_master_stop(cmd);

    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, pdMS_TO_TICKS(1000));
    i2c_cmd_link_delete(cmd);

    if (ret == ESP_OK)
    {
        *value = (msb << 8) | lsb;
    }
    return ret;
}

// 读取总线电压
float ina219_read_bus_voltage_v(i2c_port_t i2c_num, uint8_t dev_addr)
{
    uint16_t raw;
    esp_err_t ret = ina219_read_register(i2c_num, dev_addr, INA219_BUS_VOLTAGE, &raw);
    if (ret == ESP_OK)
    {
        return (raw >> 3) * 0.004f; // 每LSB=4mV
    }
    return 0.0f;
}

// 读取电流
float ina219_read_current_ma(i2c_port_t i2c_num, uint8_t dev_addr)
{
    uint16_t raw;
    esp_err_t ret = ina219_read_register(i2c_num, dev_addr, INA219_CURRENT, &raw);
    if (ret == ESP_OK)
    {
        int16_t signed_raw = (int16_t)raw;
        return signed_raw * g_currentLSB * 1000.0f; // 转换为mA
    }
    return 0.0f;
}

// 读取功率
float ina219_read_power_mw(i2c_port_t i2c_num, uint8_t dev_addr)
{
    uint16_t raw;
    esp_err_t ret = ina219_read_register(i2c_num, dev_addr, INA219_POWER, &raw);
    if (ret == ESP_OK)
    {
        return raw * 20 * g_currentLSB * 1000.0f; // 转换为mW
    }
    return 0.0f;
}

void i2c_scan(i2c_port_t i2c_num)
{
    ZYP_DEBUG("Scanning I2C bus...\n");
    for (uint8_t i = 0x08; i < 0x78; i++)
    {
        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
        i2c_master_start(cmd);
        i2c_master_write_byte(cmd, (i << 1), true); // 写模式
        i2c_master_stop(cmd);
        esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, pdMS_TO_TICKS(100));
        i2c_cmd_link_delete(cmd);
        if (ret == ESP_OK)
        {
            ZYP_DEBUG("Found device at 0x%02X\n", i);
        }
        else if (ret != ESP_OK)
        {
            ZYP_DEBUG("Error scanning address 0x%02X: %d\n", i, ret);
        }
    }
}